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Abstract 


Motion  of  the  muzzle  of  a  weapon  fired  firom  a  moving  vehicle  occurs  during  firing  because 
of  many  factors,  such  as  vibrations  caused  by  the  vehicle’s  wheels  or  the  terrain.  This  motion 
can  have  adverse  effects  on  the  capabilities  of  the  weapon  to  hit  a  target,  because  the  shooter  is 
unable  to  accurately  position  the  muzzle  of  the  weapon  onto  the  target  as  the  projectile  exits  the 
barrel.  Large,  heavy  vehicles,  such  as  the  Abrams  tank,  the  Bradley  Fighting  Vehicle,  and  the 
costly  Apache  helicopter,  have  very  expensive  gun  turrets  that  are  controlled  by  very  expensive, 
fully  stabilized  gun  sights  to  accurately  position  the  muzzle  of  the  weapon  onto  the  target. 
However,  small  and  lightweight  vehicles,  such  as  a  small  helicopter,  a  fast  attack  vehicle,  or  a 
high-mobility  multipurpose  wheeled  vehicle  (HMMWV),  carmot  justify  such  expensive  gun 
turrets  and  fully  stabilized  sights.  Therefore,  to  improve  the  accuracy  of  a  weapon  firing  fi'om 
a  small,  lightweight  vehicle,  the  U.S.  Army  Research  Laboratory  (ARL)  has  developed  the 
Inertial  Reticle  Technology  (IRT). 

This  report  presents  how  the  IRT  was  applied  to  a  5.56-mm  M16A2  rifle  firing  from  a  fast 
attack  vehicle.  The  complete  details  of  the  IRT  applied  to  a  5.56-mm  M16A2  rifle  firing  from 
a  fast  attack  vehicle  are  presented  along  with  an  analysis  of  stationary  and  moving  vehicle  live 
fire  test  data. 
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1.  Introduction 


Motion  of  the  muzzle  of  a  weapon  fired  from  a  moving  vehicle  occurs  during  firing  because 
of  many  factors,  such  as  vibrations  caused  by  the  vehicle’s  wheels  or  the  terrain.  This  motion 
can  have  adverse  effects  on  the  capabilities  of  the  weapon  to  hit  a  target,  because  the  shooter  is 
unable  to  accurately  position  the  muzzle  of  the  weapon  onto  the  target  as  the  projectile  exits  the 
barrel.  Large,  heavy  vehicles,  such  as  the  Abrams  tank,  the  Bradley  Fighting  Vehicle,  and  the 
costly  Apache  helicopter,  have  very  expensive  gun  turrets  that  are  controlled  by  very  expensive, 
fully  stabilized  gun  sights  to  accurately  position  the  muzzle  of  the  weapon  onto  the  target. 
However,  small  and  lightweight  vehicles,  such  as  a  small  helicopter,  a  fast  attack  vehicle,  or  a 
high-mobility  multipurpose  wheeled  vehicle  (HMMWV),  cannot  justify  such  expensive  gun 
turrets  and  fully  stabilized  sights.  Therefore,  to  improve  the  accuracy  of  a  weapon  firing  from  a 
small,  lightweight  vehicle,  the  U.S.  Army  Research  Laboratory  (ARL)  has  developed  the  Inertial 
Reticle  Technology  (IRT). 

2.  The  IRT  Applied  to  a  Weapon  Firing  From  a 
Lightweight  Vehicle 

Two  test  beds  were  built  as  part  of  the  IRT  program,  which  was  an  Army  Science  and 
Technology  Objective  culminating  in  December  1998.  The  initial  test  bed  was  a  fast  attack 
vehicle  on  which  the  IRT  was  integrated  with  a  5.56-mm  M16A2  rifle.  The  second  test  bed  was 
a  HMMWV  on  which  the  IRT  was  integrated  with  a  .50-cal.  M2  heavy-barrel  machine  gun. 
This  section  discusses  the  common  aspects  of  the  design,  and  the  rest  of  this  report  focuses  on 
the  fast  attack  vehicle  test  bed. 

The  IRT  replaces  the  conventional  sights  or  scope  on  the  weapon  fired  firom  a  lightweight 
vehicle  with  a  video  camera  that  is  mounted  to  the  weapon  and  a  video  display  mounted  in  a 
remote  control  panel  that  rests  on  the  gunner’s  lap  inside  the  vehicle.  The  IRT  also  replaces  the 
weapon  mount  on  the  lightweight  vehicle  with  a  remotely  operated,  lightweight,  and  inexpensive 
weapon  positioner  or  turret. 
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the  IRT  test  beds,  the  weapon  positioner  drives  the  weapon  in  both  the  elevation  and 
azimuth  directions  by  means  of  two  inexpensive  low-power  stepper  motors.  Two  shaft  angle 
encoders  are  attached  to  the  elevation  and  azimuth  axes  of  the  weapon  positioner  to  measure  the 
relative  angular  displacements  of  the  weapon  relative  to  the  weapon  platform.  Three  orthogonal 
angular  rate  sensors  are  mounted  on  the  weapon  platform.  The  outputs  of  the  rate  sensors  are 
integrated  to  provide  the  angular  displacements  of  the  weapon  platform  in  the  pitch,  yaw,  and 
roll  directions.  On  the  fast  attack  vehicle,  a  simple  wheel  counter,  mounted  on  the  vehicle, 
counts  wheel  rotations,  which  allows  the  calculation  of  vehicle  translation  relative  to  the  target. 
A  revolution  counter  on  the  speedometer  cable  performs  the  same  task  on  the  HMMWV. 

The  initial  range  of  the  vehicle  to  the  target  is  put  into  the  computer  manually  or  from  a  range 
finder  mounted  on  the  weapon.  A  continuous  readout  of  the  range  is  shown  on  the  operator’s 
display  in  the  remote  control  panel  that  rests  on  the  gunner’s  lap  inside  the  vehicle.  A  small 
computer  is  used  to  generate  two  electronic  pointers  that  can  be  overlaid  on  the  video  image. 
The  first  pointer  is  a  dot  that  is  aligned  with  the  barrel  centerline  of  the  weapon  and,  thus, 
represents  the  aim  point.  With  inputs  from  the  range  finder,  the  wheel  counter,  and  the  shaft 
angle  encoders,  the  aim  point  is  ballistically  corrected  for  range  and  lag  angles  and  becomes  the 
ballistic  solution.  The  second  pointer  is  a  crosshair  referred  to  as  the  inertial  reticle.  This  reticle 
is  driven  in  opposition  to  the  weapon  and  vehicle  motions,  as  measured  by  the  shaft  angle 
encoders,  integrated  rate  sensors,  and  the  wheel  counter,  so  that  the  inertia  reticle  appears  to 
remain  fixed  relative  to  the  target,  even  though  the  weapon  and  vehicle  might  be  moving.  Even 
though  the  video  scene  may  be  moving  around  significantly,  there  is  no  relative  motion  between 
the  inertial  reticle  and  the  target.  This  makes  it  easy  to  position  the  inertial  reticle  over  the 
desired  target  using  a  joystick  mounted  on  the  remote  control  panel. 

Once  the  initial  range  to  the  target  is  put  into  the  computer  and  the  inertial  reticle  is 
accurately  placed  over  the  desired  target,  the  computer  continuously  measures  the  difference  in 
the  position  of  the  inertial  reticle  relative  to  the  aim  point.  This  error  signal  is  used  to  drive  the 
stepper  motors  to  position  the  aim  point  over  the  inertial  reticle.  As  the  aim  point  and  the  inertial 
reticle  are  aligned,  a  prediction  algorithm  on  the  computer  calculates  die  precise  firing  time. 
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ensuring  that  the  projectile  exits  the  muzzle  as  the  ballistic  solution  is  aligned  with  the  intertial 
reticle.  Assuming  that  the  gunner  has  enabled  the  system  to  fire,  the  precise  firing  is 
accomplished  by  means  of  an  electric  solenoid  that  is  attached  to  the  trigger  of  the  weapon.  To 
simplify  the  display  for  the  gunner,  the  ballistic  solution  is  typically  not  displayed,  and  the 
gunner’s  tasks  are  to  select  the  target,  enter  the  range,  and  keep  the  inertial  reticle  on  target. 

3.  The  IRT  AppUed  to  a  5.56-mm  M16A2  Rifle  Firin® 
From  a  Fast  Attack  Vehicle 

The  IRT  was  applied  to  a  5.56-mm  M16A2  rifle  firing  from  a  fast  attack  vehicle,  as  shown  in 
Figures  1  and  2.  The  5.56-mm  M16A2  rifle  was  fitted  with  a  Sony  (EVI-330T)  CCD  camera 
block.  The  Sony  (EVI-330T)  camera  block  has  a  12x  optical  zoom,  auto  focus  lens.  It  also  has  a 
2x  electronic  zoom,  which  when  combined  with  the  12x  optical  zoom  gives  a  video  image  that  is 
equivalent  to  the  image  seen  through  a  conventional  12x  scope.  The  video  image  from  the 
camera  block  is  viewed  by  the  gunner  on  a  Sony  flat-panel  display  monitor  (FMD-402A) 
mounted  in  the  remote  control  panel. 


Figure  1.  The  IRT  Applied  to  an  M16A2  Rifle  Firing  From  a  Fast  Attack  Vehicle 
(Right  Side  View). 
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Figure  2.  The  IRT  Applied  to  an  M16A2  Rifle  Firing  From  a  Fast  Attack  Vehicle 
(Left  Side  View). 


The  5.56-inni  M16A2  rifle  is  mounted  on  two  Aerotech  (ART  312)  rotary  positioning  stages. 
One  stage  is  used  for  the  elevation  direction  and  the  second  stage  is  used  for  the  azimuth 
direction.  Both  stages  are  driven  by  stepper  motors  that  are  powered  by  Dynacron  (DM  8010) 
microstepping  translators.  Two  Itek  LSI  Micro  Series  (mS/16/23K)  shaft  angle  encoders  measure 
the  relative  angular  displacements  between  the  weapon  and  the  weapon  platform  in  azimuth  and 
elevation.  The  shaft  angle  encoders  are  mounted  inside  of  the  rotary  positioning  stages,  and  their 
ou^ut  shafts  are  attached  to  the  elevation  and  the  azimuth  axis  of  the  weapon  platform.  Three 
Systron  Dormer  quartz  rate  sensors  (QRS-1 1-0010-101)  mounted  to  the  weapon  platform 
measure  the  angular  displacements  of  the  weapon  platform  in  elevation,  azimuth,  and  roll.  A 
simple  wheel  counter  that  was  designed  and  built  at  ARL  measures  vehicle  translation. 

The  initial  range  of  the  vehicle  to  the  target  is  measured  using  a  Helios  range  finder  that  is 
mounted  just  above  the  CCD  camera  block.  The  range  ftnder  is  controlled  remotely  from  inside 
the  vehicle  by  switches  on  the  remote  control  panel.  Once  the  initial  range  to  the  target  is  put 
into  the  computer  and  the  inertial  reticle  is  accurately  placed  over  the  desired  target,  the  target 
designator  switch  on  the  gunner’s  control  panel  is  momentarily  engaged.  At  this  time,  the  initial 
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position  of  the  target  relative  to  the  inertial  reticle  is  determined  in  inertial  coordinates. 
Theoretically,  once  the  inertial  reticle  is  positioned  accurately  over  the  target,  it  should  stay  there 
indefinitely.  However,  due  to  random  walk  in  the  quartz  rate  sensors,  the  inertial  reticle  will 
drift  off  from  the  target  after  several  seconds.  The  inertial  reticle  can  be  easily  repositioned  over 
the  target  by  slight  movements  of  the  joystick.  If  the  drift  becomes  large  enough  to  get  outside 
of  the  range  of  the  joystick  correction,  a  switch  on  the  gunner’s  control  panel  can  be  turned  on, 
switching  the  joystick  operation  from  the  displacement  mode  to  the  velocity  mode  and  giving  it 
an  endless  correction  range.  A  second  switch  on  the  gunner’s  control  panel  can  also  be  engaged 
when  the  joystick  is  in  velocity  mode.  This  causes  the  stepper  motors  of  the  weapon  positioner 
to  move  at  a  much  faster  rate,  which  facilitates  getting  the  target  in  the  field  of  view. 

Control  of  firing  the  weapon  is  accomplished  by  means  of  an  arm  switch  and  a  fire  button 
that  are  mounted  on  the  gunner’s  control  panel.  Once  the  arm  switch  is  turned  on  and  the  gunner 
is  satisfied  with  the  position  of  the  inertial  reticle  over  the  desired  target,  then  the  gunner 
depresses  the  fire  button  and  holds  it  depressed  to  enable  the  electrical  firing  solenoid.  Using  the 
ballistic  solution,  the  weapon  automatically  fires  such  that  the  projectile  exit  from  the  barrel 
occurs  when  the  muzzle  of  the  weapon  is  properly  aligned  on  the  target. 

The  integration  of  the  quartz  rate  sensors  signals,  the  reading  of  the  wheel  counter,  the 
reading  of  the  shaft  angle  encoders,  the  determination  of  the  target  position  in  inertial  space,  the 
control  of  the  weapon  positioner,  the  generation  of  the  predictive  fire  control  algorithm,  and  the 
firing  of  the  weapon  are  accomplished  by  a  small  WinSystems  486  SLC  computer  and  power 
supply.  The  generation  of  the  electronic  pointers  is  accomplished  by  a  small  386  SX  computer 
that  is  fed  directly  into  the  WinSystems  486  SLC  computer. 

The  complete  computer  programs  for  both  of  the  computers  are  presented  in  the  Appendix. 
The  gunner’s  control  panel  is  shown  in  Figure  3.  The  video  image  as  seen  on  the  monitor  in  the 
gunner’s  control  panel  is  shown  in  Figure  4.  The  range  to  the  target  is  shown  in  the  upper  left 
comer.  The  diameter  of  the  black  target  in  the  center  of  the  video  image  is  20.3  cm. 
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Figure  4.  Video  Image  of  20.3-cm  Target  at  417  m  (Monitor  in  Remote  Control  Panel) 
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4.  Indoor  Testing  of  the  IRT  Applied  to  a  5.56-nun 
M16A2  Rifle 

Prior  to  any  long-range  testing  of  the  IRT  applied  to  a  5.56-mm  M16A2  rifle  firing  from  a 
fast  attack  vehicle,  extensive  short-range  testing  was  done  in  the  indoor  range  in  Building  390  at 
ARL.  Over  100  rounds  were  fired  with  the  weapon  positioner  mounted  to  a  rigid  plate  to 
determine  if  the  weapon  positioner,  the  video  camera,  the  shaft  angle  encoders,  and  the  quartz 
rate  sensors  could  withstand  the  shock  from  firing.  Accuracy  measurements  were  also  taken 
during  the  initial  testing  for  each  round  fired.  The  accuracy  acceptance  specification  for  M855 
ammunition,  which  was  used  in  the  initial  testing,  converts  to  an  average  standard  deviation  of 
.28  mil  in  both  the  elevation  and  the  azimuth  directions.  The  average  standard  deviations  in  the 
elevation  and  the  azimuth  directions  for  several  10-round  groups  fired  with  the  weapon 
positioner  mounted  to  a  rigid  plate  were  .26  mil  and  .24  mil,  respectively.  Since  the  average 
standard  deviation  for  the  experiments  with  the  weapon  positioner  mounted  to  a  rigid  plate  were 
essentially  the  same  as  the  accuracy  acceptance  specification,  it  was  felt  that  the  IRT  integrated 
with  the  5.56-mm  M16A2  rifle  was  achieving  its  maximum  accuracy  performance  for  these 
conditions  and  no  further  short-range  indoor  experiments  were  performed.  There  was  also  no 
noticeable  damage  to  the  weapon  positioner,  the  video  camera,  the  shaft  angle  encoders,  or  the 
quartz  rate  sensors  after  firing  over  100  rounds. 

5.  Initial  Long-Range  Outdoor  Testing  of  the  IRT 
Applied  to  a  5.56-nMn  M16A2  Rifle  Firing  From  a  Fast 
Attack  Vehicle 

After  the  indoor  testing  was  completed,  initial  long-range  outdoor  firing  of  the  IRT  applied  to 
the  5.56-mm  M16A2  IRT  test  bed  was  mounted  on  a  fast  attack  vehicle  and  initial  long-range 
outdoor  firing  experiments  were  done  at  the  U.S.  Army  Aberdeen  Test  Center  (ATC)  H-Field 
test  facility  at  the  Edgewood  Area  of  Aberdeen  Proving  Ground.  Before  any  firings  from  a 
moving  vehicle  were  done,  firings  from  a  stationary  vehicle  were  made  at  a  400-m  target.  The 
average  standard  deviations  in  the  elevation  and  the  azimuth  directions  for  several  five-round 
groups  of  M855  ammunition  fired  semiautomatically  by  the  gunner  from  inside  the  stationary 
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vehicle  were  .29  mil  and  .30  mil,  respectively.  Since  the  average  standard  deviations  for  the 
firings  by  the  gunner  from  inside  the  stationary  vehicle  were  essentially  the  same  as  the  accuracy 
acceptance  specification  of  .28  mil,  it  was  felt  that  the  IRT  integrated  with  a  5.56-mm  M16A2 
rifle  was  achieving  its  maximum  accuracy  performance  for  these  conditions  and  no  further 
stationary  long-range  experiments  were  required,  and  firing-on-the-move  experiments  were 
initiated. 

After  completing  the  stationary  vehicle  firings  at  the  400-m  target,  the  firings  were  repeated 
with  the  gunner  firing  firom  inside  the  vehicle  while  the  vehicle  was  moving  toward  the  target. 
Five-round  groups  were  fired  semiautomatically  by  the  gunner  at  about  1-s  intervals,  while  the 
vehicle  was  traveling  at  16  kph  down  a  gravel  road  toward  a  400-m  target.  The  average  standard 
deviations  in  the  elevation  and  the  azimuth  directions  for  several  five-round  groups  of  M855 
ammunition  fired  semiautomatically  by  the  gunner  from  inside  the  moving  vehicle  were  .93  mil 
and  .87  mil,  respectively.  Since  the  standard  deviations  were  considerably  higher  than  those 
obtained  in  the  stationary  vehicle  firings,  the  firings  were  stopped  to  determine  why  the  standard 
deviations  were  so  much  higher. 

In  reviewing  the  video  tape  of  the  inertial  reticle  taken  during  the  firings  firom  the  moving 
vehicle,  it  was  determined  that  a  high-frequency  oscillation  of  about  25  Hz  was  being  transmitted 
from  the  frame  of  the  fast  attack  vehicle  into  the  weapon  platform  as  the  fast  attack  vehicle  was 
traveling  down  the  gravel  road  at  16  kph.  The  weapon  controller  and  the  IRT  sensors  easily 
handled  the  25-Hz  oscillations  and  held  the  reticle  over  the  aim  point,  but  the  video  image  was 
moving  so  rapidly  that  multiple  reticles  appeared  that  made  it  very  difficult  to  accurately  hold  the 
inertial  reticle  on  the  target. 

The  25-Hz  oscillation  also  caused  a  serious  problem  with  the  firing  predictor,  because  the 
time  interval  from  the  firing  pulse  to  the  projectile  exit  firom  the  gun  barrel  was  30  ms  for  the 
hammer-fired  5.56-mm  M16A2  rifle.  At  25  Hz,  the  firing  predictor  could  not  predict  reliably  out 
to  30  ms  and  there  were  many  instances  when  the  muzzle  of  the  weapon  was  not  pointing  at  the 
target  when  the  projectile  exited  the  gun  barrel.  To  prevent  the  25-Hz  oscillation  in  the  frame  of 
the  fast  attack  vehicle  from  being  transmitted  to  the  weapon  platform,  an  inexpensive  isolation 
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mount  was  designed  and  built  and  placed  between  the  frame  of  the  fast  attack  vehicle  and  the 
weapon  platform.  By  using  two  roller  bearings,  two  swivel  bearings,  several  light  springs,  three 
oil-filled  dashpots,  and  extra  weights,  the  frequency  of  the  weapon  platform  was  reduced  to 
about  3  Hz  in  the  elevation,  the  azimuth,  and  the  roll  directions  when  the  fast  attack  vehicle  was 
traveling  at  16  kph  down  the  gravel  road.  At  3  Hz,  there  were  no  multiple  inertial  reticles  on  the 
video  image  and  the  inertial  reticle  could  easily  be  held  on  target.  The  firing  predictor  was  also 
easily  able  to  predict  reliably  out  to  30  ms.  The  isolation  mount  can  be  seen  in  Figure  5  between 
the  frame  of  the  fast  attack  vehicle  and  the  weapon  platform. 


Figure  5.  The  Isolation  Mount  Between  the  Frame  of  the  Fast  Attack  Vehicle  and  the 
Weapon  Platform. 


6.  Final  Long-Range  Outdoor  Testing  of  the  IRT  Applied 
to  a  5.56-mm  M16A2  Rifle  Firing  From  a  Fast  Attack 
Vehicle 

After  the  initial  long-range  outdoor  testing  was  completed  and  the  isolation  mount  was 
installed  and  tested  extensively  in  nonfiring  rans  at  16  Iqph  down  a  gravel  road,  final  long-range 
outdoor  testing  of  the  IRT  applied  to  an  M16A2  rifle  firing  from  a  fast  attack  vehicle  was  done  at 
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the  H-Field  test  facility  at  the  Edgewood  Area  of  Aberdeen  Proving  Ground.  The  firing 
experiments  from  a  moving  vehicle  done  in  the  initial  long-range  outdoor  testing  of  the  IRT  were 
repeated  on  the  same  firing  range.  Ten  rounds  were  fired  semiautomatically  by  the  gunner  from 
inside  the  vehicle  at  about  1-s  intervals  while  the  vehicle  was  moving  at  16  kph  down  the  same 
gravel  road  toward  the  400-m  target  The  average  standard  deviations  in  the  elevation  and  the 
azimuth  directions  for  several  10-round  groups  of  M855  ammunition  fired  semiautomatically  by 
the  gunner  fi:om  inside  the  moving  vehicle  were  .47  mil  and  .43  mil,  respectively.  The  extreme 
spread  for  the  firings  was  43  cm. 

In  reviewing  the  videotape  of  the  inertial  reticle  taken  during  the  firing  experiments  from  the 
moving  vehicle,  it  was  determined  that  the  weapon  platform  was  oscillating  at  a  frequency  of 
about  3  Hz.  A  check  of  the  firing  time  data  also  taken  during  the  firing  experiments  showed  that 
there  were  no  instances  when  the  muzzle  of  the  weapon  was  not  pointing  at  the  target  when  the 
projectile  exited  the  gun  barrel.  Since  the  average  standard  deviations  for  the  firings  by  the 
gunner  from  inside  the  moving  vehicle  were  only  slightly  higher  than  those  fired  from  the 
stationary  vehicle,  it  was  felt  that  the  IRT  applied  to  an  M16A2  rifle  firing  from  a  fast  attack 
vehicle  was  achieving  its  optimum  performance  in  accuracy  for  this  scenario.  Once  the  firings 
from  the  moving  vehicle  while  driving  toward  the  400-m  target  were  completed,  the  target  was 
placed  400-m  off  to  the  side  of  the  vehicle  and  the  firing  experiments  were  repeated  with  the 
gunner  firing  from  inside  the  moving  vehicle  and  the  weapon  pointing  over  the  right  and  left 
sides  of  the  vehicle  while  it  traversed  parallel  to  the  target  along  a  gravel  road  at  16  lq)h.  The 
IRT  held  the  inertial  reticle  on  the  target  and  put  in  the  correct  amount  of  lag  angle  so  that  the 
projectiles  hit  on  target.  The  average  standard  deviations  for  the  elevation  and  the  azimuth 
directions  for  several  10-round  groups  of  M855  ammunition  fired  semiautomatically  by  the 
gunner  from  inside  the  moving  vehicle  were  .48  mil  and  .50  mil,  respectively.  The  extreme 
spread  for  the  firings  was  48  cm.  The  average  standard  deviations  were  essentially  the  same  as 
those  for  the  previous  firings  for  the  vehicle  traveling  straight  toward  the  target. 

To  check  out  the  capability  of  the  IRT  to  fire  at  a  moving  target,  if  data  on  the  movement  of 
the  target  were  available  to  the  IRT  computer,  the  first  firing  experiments  of  firing  at  a  400-m 
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target  while  the  vehicle  was  traveling  along  a  gravel  road  at  16  kph  toward  the  target  were 
repeated.  However,  in  the  new  experiments,  the  target  was  movmg  left  to  right  at  16  kph. 
Ten-round  groups  were  fired  semiautomatically  by  the  gunner  from  inside  the  vehicle  at  about 
1-s  intervals  while  the  vehicle  was  moving  down  the  gravel  road  at  16  kph  toward  the  400-m 
target.  The  IRT  held  the  inertial  reticle  on  the  target  and  put  in  the  correct  amount  of  lead  angle 
so  that  the  projectiles  hit  on  target.  The  average  standard  deviations  in  the  elevation  and  the 
azimuth  directions  for  several  10-round  groups  of  M855  ammunition  fired  semiautomatically  by 
the  gunner  from  inside  the  moving  vehicle  were  .40  mil  and  .45  mil,  respectively.  The  extreme 
spread  for  the  firings  was  46  cm.  These  average  standard  deviations  were  essentially  the  same  as 
those  for  the  previous  firings  for  the  vehicle  traveling  straight  toward  the  target. 


7.  Conclusions 

(1)  The  IRT  applied  to  an  M16A2  rifle  firing  from  over  the  front  of  a  fast  attack  vehicle 
improved  the  accuracy  to  such  an  extent  that  a  test  engineer  was  able  to  keep  10-round 
groups  of  M855  ammunition  to  within  a  43-cm  circle,  which  was  centered  on  the  target, 
while  firing  at  the  rate  of  about  60  rd/min  from  inside  the  vehicle  while  it  was  moving  at  16 

kph  toward  a  400-m  target. 

(2)  The  IRT  applied  to  an  M16A2  rifle  firing  from  over  the  side  of  a  fast  attack  vehicle  improved 
the  accuracy  to  such  an  extent  that  a  test  engineer  was  able  to  keep  10-round  groups  of  M855 
ammunition  to  within  a  48-cm  circle,  which  was  centered  on  the  target,  while  firing  at  the 
rate  of  about  60  rd/min  from  inside  the  vehicle  while  it  was  moving  at  16  kph  parallel  to  a 

400-m  target. 

(3)  The  IRT  applied  to  an  M16A2  rifle  firing  from  over  the  front  of  a  fast  attack  vehicle 
improved  the  accuracy  to  such  an  extent  that  a  test  engineer  was  able  to  keep  10-round 
groups  of  M855  ammunition  to  within  a  46-cm  circle,  which  was  centered  on  the  target, 
while  firing  at  the  rate  of  about  60  rd/min  from  inside  the  vehicle  while  it  was  moving  at 
16  kph  toward  a  400-m  target  that  was  moving  left  to  right  at  16  kph. 
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Intentionally  left  blank. 
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Appendix: 

Computer  Programs* 


’  Program  Charlie4  is  the  computer  program  for  the  386  SX  computer.  Program  Z  is  the  computer  program  for  the 
WinSystem  486  SLC  computer. 


Intentionally  left  blank. 
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Program  Charlie4;  {May  3rd,  1994, 09:54  A.M.,  November  3, 1994} 
uses  {Video  Charlie) 

graph,  crt; 

type  alO  =  array[0..9]  of  word; 

var  A  :alO; 

Gd,  Gm  :integer, 
pO,  pi  rboolean; 

ccc,ddd,i,j,k,l  :byte; 
vw,www  :word; 
xl  1  ,xl2,x21  ,x22,yl  1  ,yl2,y21  ,y22:word; 
r0,rl  ,q0,al  1  ,bl  1 :  word; 

Procedure  Initialize; 
begin 
asm 

mov  dx,74bh 

mov  al,  09bh  {Ports  A,  B  and  C  all  input) 
out  dx,al 
end; 

’  xll:=0;  xl2:=0;  yll:=0;  yl2:=0;  x21:=0;  x22:=0;  y21:=0;  y22:=0; 
all:=320;  bll:=100;  a[0]:=0;  a[l]:=0;  a[2]:=0;  a[3]:=0; 
end; 


Procedure  Plot_It  (x,y:byte;  var  v:byte); 
begin 

SetColor(Black); 

r0:=x;rl:=x+7; 

q0:=y; 

Iine(r0,q04:l,q0); 

r0:=x;rl:=x+7; 

q0:=y+l; 

Ime(r0,q0,rl  ,q0); 

r0:=x;rl:=x+7; 

q0:=y+2; 

Ime(r0,q0,rl  ,q0); 
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r0:=x;rl:=x+7; 

q0;-y+3; 

Iine(r0,q0^1,q0); 

rO:=x;rl:=x+7; 

q0:=y+4; 

Iine(r0,q0^1,q0); 

r0:=x;rl:=x+7; 

q0:=y+5; 

Iine(r0,q0,rl,q0); 

iO:=x;rl:=x+7; 

q0:=y+6; 

Iine(r0,q0^1,q0); 

setcolor(white); 
ifv<5then 
begin  {0-4} 
case  V  of 

0:outtextxy(x,y,'0'); 
1  :outtextxy(x,y 
2:outtextxy(x,y,'2'); 
3:outtextxy(x,y,'30; 
4:outtextxy(x,y,'4'); 
end 
end 
else 

begin  {5-9} 
case  V  of 

5:outtextxy(x,y,'5'); 

6:outtextxy(x,y,'6'); 

7:outtextxy(x,y,'7'); 

8:outtextxy(x,y,'8'); 

9:outtextxy(x,y,'9'); 

end 

end 

end; 


(**********^!**:jc****  *************t**:iiiti**'^ 

Procedure  GetData(var  ddd,ccc:byte;  var  vw,www:word;  var  pO,pl:boolean); 
begin 

{sxxx  xxxx  xccc  cddd  dddddddd} 
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asm  {$G+} 
mov  dx,749h 

@0:  inal,dx  ;  movcl,al  {xccc  cddd  >  cl} 

decdx  ;  inal,dx  ;  movch,al  {dddd  dddd  >  ch> 

incdx  ;  inal,dx  {xccc  dddd  >  al} 

andal,cl  ;  andal,080h  ;jz@0 


les  bx,DDD 
mov  al,cl 
ror  al,3 
and  al,0fh 
mov  es:[bx],al 


(xccc  cddd  >  al> 
{dddx  cccc  >  al> 

{0000  cccc  >  al) 
{0000  cccc  >  DDD) 


lesbx,VW  ;  mov  es;[bx],ch 
mov  al,cl  ;  and  al,07h  ; 
mov  es:[bx+l],al 


{dddd  dddd  >  VW> 
{0000  0ddd>al> 

{0000  Oddd  >  vw+l } 


les  bx,CCC  ;  {Bit  7  of  port  74ah  is  connected  to  > 
mov  dx,074ah  ;  {the  external  video  sync.) 
in  al,dx  ;  {No  other  bits  are  used.  > 
mov  es:[bx],al 


A[DDD]  :=  VW; 
begin 
pi  :=  pO; 

if  ((CCC  and  128)  >  0)  then  pO  :=  trae  else  pO  :=  false; 

if  ( (pO  =  true)  and  (pi  =  false)  )  then 
begin 

begin 

VW  :=  A[4]; 

i  ;=  VW  div  1000;  vw  :=  vw  -  i*1000;  plot_it(10,10,i); 
j  :=  vw  div  100;  vw  :=  wv  -  j*  100;  plot_it(20,10,j); 
k  :=  vw  div  10;  vw  :=  wv  -  k*  10;  plot_it(30,10Jic); 

1  :=  vw;  plot_it(40,10d); 

end; 


begin 

SetColor(Black); 
line(xl  1  ,y  1 1  ,x  1 2,y  1 2) ; 
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Iine(x21  ,y21  ,x22,y22); 

Iine(x21-2,y21  ,x22-2,y22); 
Iine(x21+2,y21,x22+2,y22); 

VW  :=  A[2]; 

if  VW  >  638  then  vw  :=  638; 
xll  :=vw-20;ifxll<  lthenxll:=  1; 
xl2  :=  vw  +  20;  if  xl2>  638  then  xl2:=  638; 
x21  :=  vw;  x22  :=  wv; 


WWW:=A[3]; 

if  WWW  >  198  then  www  :=  198; 
y21  :=  WWW  -  8;  if  y21<  ltheny21:=  1; 
y22  :=  www  +  8;  if  y22>  198  then  y22:=  198; 
yl  1  :=  www;  yl2  :=  www; 


SetColor(White); 
Iine(xll,yll,xl2,yl2); 
line(x2 1  ,y2 1  ,x22,y22); 
line(x21 -2,y2U22-2,y22); 
line(x2 1  +2,y2 1  ,x22+2,y22) 
end; 

begin 

SetColor(Black); 

line(al  1-20, bl  1-8, al  1+20, bl  1-8); 
line(al  l-20,bl  l+8,al  l+8,bl  1+8); 


VW:=A[0]; 

if  vw  >  630  then  vw  :=  630; 
if  vw  <  8  then  vw  :=  8; 
al  1  :=  vw; 


WWW:=A[1]; 

if  WWW  >190  then  www  :=  190; 
if  WWW  <  8  then  WWW  :=  8; 
bl  1  :=  www; 

{if(dddand2  =  0} 

SetColor(White); 

line(al  l-20,bl  l-8,al  l+20,bl  1-8); 

line(al  l-20,bl  l+8,al  l+8,bl  1+8); 
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end;  {if  ( (pO  =  true)  and  (pi  =  false)  )> 
end; 

end;  {procedure} 


^*  *****************  ********************) 
begin  {Main} 


Gd:=detect; 

Initgraph  (Gd,  Gm,  'c:\tp\bgi'); 
if  graphresult  <>  grOk  then 
begin 

writeln('Cannot  file  graphics  files.  Press  any  key  to  continue.'); 
readln;  halt  (1); 
end; 


(* - initial  values - *) 

Initialize;  p0:=  true; 

while  keypressed  =  false  do  GetData(ddd,ccc,vw,www,pO,pl); 
closegraph; 
end. 
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Program  Z;  {September  14, 1992,  June  21, 1993,  March  24, 1994} 

{N+}  {March  31 , 1994  -  Predict  40  ms,  cycle  2  ms} 

{April  25, 1994  -  used  with  video  reticle  genrator} 

{February  7, 1 995,  February  2 1 , 1 995  } 

{April  30, 1996,  May  8, 1996} 

{May  14, 1996  used  with  GPS} 

{August  9, 1996  -  new  processor  board  (33MhZ),  new  dt,  new  time;  6.0ms} 
{August  9, 1996  -  new  delays  for  the  Laser  Range  Finder} 

{October  1996  -  Rewritten  and  updated.  Includes  wheel  geometry} 
{January  1997  -  New  predictor  -  outputted  to  DAC  ports/with  firing  pulse} 
{November  4,1997  -  Camera  stabilization} 

{November  20,1997-  New  camera  stabilization} 

{BFCs  denotes  Body  Fixed  Coordinates,  ICs  denotes  Inertial  Coordinates} 

Uses  Graph,Crt;  {Port  assignments:  050h  to  057h} 


Const  B_V=20;  B_H=30;  B_V2=B_V  div  2;  B_H2=B_H  div  2; 

Const  bit0*0;bitl=l  ;bit2=4;bit3=8;bit4=16;bit5=32;bit6=64;bit7=128; 

Const  EarthRadius  =  6369537.34;  saeo_c  =  1750;  saep_c=  -440; 

WheelToSight_X  =  1.2;  WheelToSight_Y  =  1.0;  WheelToSight_Z  =  2.0;  {BFC} 
ConversionToRadians  =  2*pi/65536;  NoOfConversionsPerSecond  =  3500; 
Coeffl=1.05*(10/9)*(pi/2)/131071;  {100  degrees  per  second  >  2**17-1 } 
ANG=0.04997558594; 

Type 

seal  >=  double; 

r33  =  array[l  ..3,1  ..3]  of  seal;  i88  =  array[0..7,0..7]  of  byte; 

r8  =  array[0..7]  of  seal; 

i8  =  array[0..7]  of  byte; 

r3  =  array[1..3]  of  seal; 

arl28b  =  array[1..150]  of  byte; 

i3  =array[1..3]  of  integer; 

alO  =  array[0..10]  of  seal;  var  r,v;al0; 

Var 

ww,swz,teed*ortCOJ*ortCl,flop  :  byte; 
s  :  arl28b;  {Supports  the  GPS} 

c_azi,c_ele,  CosO,SmO,CosP,SInP ;  Seal; 
xc,yc,  gd,  gm,  imi  ,e  :  integer; 

Fir,FirP,Error,  GPS,  WC,  Counter  :  boolean; 
abcrboolean; 

nnn  :  i8; 

tau,  we0,wel,we2,we3,we4,we5,we6  :seal; 

Range,  Azi0,Azil,Ele0,B[el,Temp,Slew :  seal; 

B1,B2,  dt,  suml,  sum2,  wx,  wy,  wz  :  seal;  {Body  fixed  angular  rates} 
C_Joy_0,C_Joy_P,Pl,P2,temp2,  SE  :  seal; 

Port7Bit6,  Port7In,  BI0,BIl,Bib,  sam :  byte; 
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channel,  n3,  n2,  nl,  nO  :  byte;  {Raw  sensor  inputs} 
port6,  Port9_In,  n7,  n6,  n5,  n4  :  byte;  {Raw  sensor  inputs) 
PortB_lhJ»7InP,  j,  k,  cnt,  P7In,  PCIn :  byte;  {couters  and  flag) 

Looper,  Delay,  wrd  :  Word; 

nn:i88;  {Raw  sensor  inputs) 
we7,  LLL,  Count  :  word; 
wappp,  wapp,wap  :  seal; 

weppp,  wepp,wep  :  seal; 

ii_c_azi,  conl,con2,alpha  :  seal; 

Dist,  Teinp_word,app,bpp :  word; 

SaeO,  SaeP,SaeO_,  SaeP_  :  seal; 

IntO,IntP,  JoyO,  JoyP,  Speed,err,zse  :  seal; 
vxx,  vyy,  vzz,dxx,dyy,dzz,dx,dy,dz  :  seal; 

a  :  r33;  {Transformation  matrix) 
i_c_ele,i_c_azi,  xi,  et,  ze,  ch  :  seal;  {Quaterian  vriables) 
c_a_drift,c_e_drift,  xidp,etdp,zedp,chdp:  seal;  {Predicted  derivatives) 
D_Az0,D_E10,  xidO,  etdO,  zedO,  chdO  :  seal;  {Previous  derivatives) 
rr,  ss,  tt,  XX,  yy,  zz,  dt2  :  seal;  {Normalized  time  step) 
irr,sss,ttt,xxx,yyy,zzz,D_Azl,D_Ell  :  seal;  {Body  fixed  angular  rates) 
xa,ya,za,xb,yb,zb  :  seal;  {Body  fixed  linear  accelerations) 

zO,zl  ,z2,z3,z4,d0,dl  ,d2,d3,d4,fff  :boolean; 

c_ele_f,c_azi_f,  azi_lim,  ele_lim  :  seal; 

TimeOfFlight,xw,  yw,  zw,  t,azza,azze  :  seal;  {Defines  point  1,  previous) 
DriftX,  DriftY,  DriftZ,  sum  :  seal; 
JoyP_,JoyO_,o,oo,ooo,p,pp,ppp,ao,ap  :  seal; 
Y09,YP9,YO,YP,SO,SP,jolm,jane  :  seal; 
aa,bb,cc,dd,ee,aabb,ccdd,  col  :  r3; 
the,phi,alt,  zzzz  :  real; 

indx  :  i3; 


Procedure  PutP3(  var  ArByte);  begin  asm  mov  dx,233h;  les  bx.  A;  mov  al,es:[bx];  out  dx,al 
end  end; 

Procedure  PutP6(  var  A:Byte);  begin  asm  mov  dx,236h;  les  bx.  A;  mov  al,es:[bx];  out  dx,al 
end  end; 

Procedure  D;  var  i,j:  byte; 

begin  for  i  :=  0  to  255  do  for  j:=  0  to  255  do  begin  end  end; 

Procedure  CX100_Mode;  var  P3:byte;begin  P3;=$47;  PutP3(P3);  D  end; 

Procedure  Dis_Ovl;  var  P6:byte;  begin  P6;=$27;  PutP6(P6);  D  end; 

Procedure  C)vr_Ena;  var  P6:byte;  begin  P6:=$29;  PutP6(P6);  D  end; 

Procedure  Dis_Int;  var  P6:byte;  begin  P6:=$10;  PutP6(P6);  D  end; 

Procedure  High_Res;var  P6:byte;  begin  P6:=$0a;  PutP6(P6);  D  end; 
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Procedure  Set_Pix(var  A,B-word); 
begin  asm  {$G+} 
les  bx,B 

mov  cx,es:[bx]  {cx:  0  0  0  0  0  0  0  b8  b7  b6  b5  b4  b3  b2  bl  b0> 
les  bx,A 

mov  dx,es:[bx]  {dx:  0  0  0  0  0  0  0  a8  a7  a6  a5  a4  a3  a2  al  a0> 
roldx,l  {dx;  0  0  0  0  0  0a8a7  a6a5a4a3  a2al  aO  0} 
xor  al,al 

movah,dl  {ax;a6a5a4a3  a2ala0  0  0000  000  0} 
add  cx,ax  {cx:a6  a5  a4  a3  a2  al  aO  b8  b7  b6  b5  b4  b3  b2  bl  bO} 
moval,dh  {al:  0  0  0  0  0  0a8a7> 

and  al,03h  {al:  0  0  0  0  0  0  a8  a7> 

mov  dx,236h  {Ou^ut  the  Page_Select  bits  to  Port  6  > 

out  dx,al  {Output  to  PO,  program  the  two  "page"  bits,  PO.l,  PO.O) 

{I>>  mov  al,l;  @0:  dec  al;  jnz  @0 

mov ax.OdOOOh  {ax:  1010  0000  0000  000  0} 
mov es,ax  {es:0000  1010  0000  000  0} 
mov  al,lbh;  out  dx,al  {Ram  Enable} 

{D}  rnoval,!;  @1:  dec  al;  jnz@l 

mov  al,  29h;  out  dx,al  {OverLay_Enable} 

{D}  moval,l;  @2:  dec  al;  jnz  @2 

mov  bx,cx  {bx:a6  a5  a4  a3  a2  al  aO  b8  b7  b6  b5  b4  b3  b2  bl  bO} 
mov  al,  OOh  {ax:  ********} 
mov  es:[bx],al  {Move  pixel  value  to  video  memory} 

{D}  mov  al,l;  @3:  dec  al;  jnz  @3 

mov  al,es:[bx]  {write  to  video  ram} 
moval,lah;  outdx,al  {Ram  Disable} 
end  end; 

Procediure  ReSet_Pix(var  A,B:word); 
begin  asm  {$G+} 

les  bx,B;  mov  cx,es:[bx];  les  bx,A;  mov  dx,es:[bx];  rol  dx,l;  xor  al,al 
movah,dl;  addcx,ax;  moval,dh;  andal,03h; 
movdx,236h;  outdx,al;  moval,l;  @0:  dec  al;  jnz  @0 
mov  ax.OdOOOh;  mov  es,ax;  mov  al,lbh;  outdx,al;  rnoval.l;  @1:  dec  al;  jnz@l 
mov  al,  29h;  out  dx,al;  mov  al,l ;  @2;  dec  al;  jnz  @2 
movbx,cx;  rnoval,  Ofh  {0,1  or  15} 
mov  es:[bx],al;  mov  al,l;  @3:  dec  al;  jnz  @3 
mov  al,es:[bx];  mov  al,lah;  out  dx,al 
end  end; 
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Procedure  LineV(  xO,  yO^irword;  colonboolean); 

var  i  :word; 

begin 

for  i  ;=  0  to  n  do 
begin 

if  color  then  Set_Pix(xO,yO)  else  ReSet_Pix(xO,yO); 
yO:=yO+l; 
end; 
end; 

Procedure  LineH(  xO,  yO^irword;  colonboolean); 

var  i  :word; 

begin 

for  i  :=  0  to  n  do 
begin 

if  color  then  Set_Pix(xO,yO)  else  ReSet_Pix(xO,yO); 
xO:=xO+l; 
end; 
end; 

Procediu:e  G_Init; 
var  i:word; 
begin 

CX100_Mode;  {Places  boared  in  the  CXI 00  mode} 
Dis_C)vl;  {Display_Overlay} 

Ovr_Bia;  {C)verlay_&iable} 

Dis_Int;  {Disable  Interrupts) 

High_Res;  {Enables  High  Resolution) 
for  i:=  0  to  5 1 1  do  LineH(i,0,5 1 1  .false); 
end; 

Procedure  G_Set(  x0,y0:word;c:boolean); 
begin 

LineV(xO,  yO,  B_V,c); 

LineH(xO,  yO,  B_H,c); 

LineH(xO,  yO+B_V,  B_H,c); 

LineV(xO+B_H,yO,  B_V,c); 
end; 


PROCEDURE  LuDcmpC  n  tinteger;  VAR  a;  r33;  VAR  indx  :i3;  VAR  d  :seal) 
CONST  tiny=1.0e-20; 

VAR  k,j,imax,i:integer; 
sum, dum,big:  seal; 
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w:r3; 

BEGIN 

d;=1.0  ; 

FOR  i:=l  TO  n  DO  BEGIN 
big:-0.0; 

FOR  j:=  1  TO  n  DO  IF  (abs(a[i,j])  >  big)  THEN  big:=abs(a[i,j]); 

IF  (big=0.0)  THEN  BEGIN  END;  {if} 

w[i]:=1.0/big; 

END;  {fori} 

FOR  j:=  1  TO  n  DO  BEGIN 
IF  (j>l)  THEN  BEGIN 
FOR  i:=l  TO  j-1  DO  BEGIN 
sum:=a[i,j]; 

IF  (i>l)  THEN  BEGIN 

FOR  k:=  1  TO  i-1  DO  sum  :=sum-a[i4c]*a[k,j];  a[i,j]  :=  sum; 
END  {if  i} 

END  {fori} 

END;  {if  j} 


big: =0.0; 

FOR  i:=j  TO  n  DO  BEGIN 
sum:=  a[i,j]; 

IF  (j>l)  THEN  BEGIN 

FOR  k:=l  TO  j-1  DO  sum:=sum-a[ijc]*a[k  j];  a[i,j]:=  sum; 
END;  {if  j} 
dum:=  w[i]*abs(sum); 

IF  (dum>big)  THEN  BEGIN  big:=dum;  imax  :=i  END  {if  dum} 
END;  {fori} 

IF  (jo  imax)  THEN 
BEGIN 

FOR  k:=  1  TO  n  DO  BEGIN 

dum:=a[imaxdc];  a[imax4c]:=a[jjk];  alj,k]:=dum; 

END;  {fork} 
d:=-d;  w[imax]:=w[j]; 

END;  {ifj} 

indx|j]:=  imax; 

IF(jon)  THEN 
BEGIN 

IF(aIj,j]=0.0)  THEN  a[j j]:=tiny;  dum  :=1.0/a[j ,j]; 

FOR  i  :=  j+1  TO  n  DO  a[i,j]:=a[i,j]*dum; 
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END  {if  j} 
END;  {for  j} 


IF(a[n4i]  =0.0)  THEN  a[n,n]  :=  tiny; 
END;  {proc} 


Procedure  LuBkSb(n  :integer;  VAR  indx  :i3;  VAR  b  :r3;  VAR  a  :r33); 
VAR  j,ip,ii4:integer; 

sum:  seal; 

BEGIN 

ii:=0; 

FOR  i:=l  TO  n  DO  BEGIN 
ip:=indx[i];  sum:=b[ip];  b[ip]:=b[i]; 

IF(ii  <>  0)THEN 
BEGIN 

FOR  j:=  ii  TO  i-1  DO  sum:=sum-a[i,j]*b|j]; 

END{ifii} 

ELSE  IF  (sumo  0.0)  THEN  n:=  i; 
b[i]:“Sum; 

END;  {fori} 

FOR  i:=  n  DOWNTO  1  DO  BEGIN 
sum  :=  b[i]; 

IF  (i<n)  THEN  FOR  j:=i+l  TO  n  DO  sum  :=  sum  -a[i,j]*blj]; 
b[i]:=  sum/a[i,i]; 

END  {fori} 

END;  {procedure} 


Procedure  Mat_Inv(var  a,y:r33);  {Generates  the  inverse  matrix  of  A  in  Y} 

var  i  j,n:integer;  d:seal;  {The  matrix  A  is  destroyed} 

begin 

n:=3; 

LuDcmp(n,a,indx,d); 
for  j  :=  1  to  n  do 
begin 

for  i  :=  1  to  n  do  col[i]:=0 ; 
col[j]:=1.0; 

LuBkSb(n,indx,col,a); 
fori:=ltondo  y[i,j]:=col[i]; 
end; 
end; 
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Procedure  MATMAT  ( var  c:r33;  a,  b:r33); 

var  i  ,jJc:word;  sum:seal; 

begin 

for  i  :=1  to  3  do  for  k  :=  1  to  3  do 

begin  sum:=0;for  j:=l  to  3  do  sum:=sum+a[i,j]*blj\k];  c[idc]  :=  sum  end; 
end; 


Procedure  MATMUL(var  a,b:r3;var  c:r33); 
var  i,j:word;  sumrseal; 
begin  for  i;=  1  to  3  do  begin  sum  :=  0; 
for  j  :=  1  to  3  do  sum  :=  sum  +  b[j]*c[i,j]; 
a[i]  :=  sum  end  end; 


Procedure  MATMULInv(var  a,b:r3;var  c:r33); 
var  i,j:word;  sumrseal; 
begin  for  i:=  1  to  3  do  begin  sum  :=  0; 
for  j  1  to  3  do  sum  :=  sum  +  b[j]*clj,i]; 
a[i]  :=  sum  end  end; 


Procedure  VDIF(var  a,b,c:r3); 

var  i:word;begin  for  i:=l  to  3  do  a[i]:=b[i]-c[i]  end; 


Procedmre  CMULT(  var  a,b,c:r3); 
begin  a[l]:=b[2]*c[3]-b[3]*c[2];  a[2]:=b[3]*c[l]-b[l]*c[3]; 
a[3]:=b[l]*c[2]-b[2]*c[l]  end; 


Procedure  Norm(var  a:r3); 
var  temprseal;  i  rword; 
begin  temp  :=  0; 

for  i  :=  1  to  3  do  temp  :=  temp  +  sqr(a[i]);  temp  :=  sqrt(temp); 
for  i  :=  1  to  3  do  a[i]:=a[i]/temp  end; 


Procedure  DotProd(var  r:seal;a,b:r3); 
begin  r  :=  a[l]*b[l]+a[2]*b[2]+a[3]*b[3];  end; 
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Function  ATan  (y,x:real):real; 

var  u:real; 

begin 

if  ( (x=0)  and  (y=0) )  then  atan:=0.0  else 
begin 

if  (abs(x)  <  abs(y))  then 
begin  {abs(x)  <  abs(y)} 
u: =arctan(abs(x/y)); 
if  x<0  then 

begin  {x<0>  if  y>0  then  atan:=pi/2+u  else  atan:=-pi/2-u  end  else 
begin  {x>0}  if  y>0  then  atan:=pi/2-u  else  atan:=-pi/2+u  end 
end  else 

begin  {abs(x)  >=  abs(y)} 
u:=  arctan(abs(y/x)); 
if  x<0  then 

begin  {x<0}  if  y>0  then  atan:=pi  -u  else  atan:=  -pi  +u  end  else 
begin  {x>0}  if  y>0  then  atan  :=  u  else  atan:=  -u  end 
end 
end 
end; 


Procedure  Mat(var  xi,et,ze,ch:seal;  var  a:r33); 
var  ze2,  et2,  xi2,  ch2,  ze_et,  ze_xi,  ze_ch,  xi_et,  et_ch,  xi_ch:seal; 
begin  {calculates  elements  of  the  transformation  matrix} 
ze2  :=  ze*ze;  xi2  :=  xi*xi;  et2  :=  et*et;  ch2  :=  ch*ch;  et_ch:=et*ch; 
ze_et:=ze*et;  ze_xi:=ze*xi;  ze_ch:=ze*ch;  xi_et:=et*xi;  xi_ch:=xi*ch; 

a[l,l]:=xi2-et2-ze2+ch2;a[l^]:=2*(xi_et+ze_ch);a[l,3]:=  2*(ze_xi-et_ch); 
a[2,l]:=2*(xi_et-ze_ch);a[2,2]:=-xi2+et2-ze2+ch2;a[2,3]:=2*(ze_et+xi_ch); 
a[3,l]:=2*(ze_xi+et_ch);a[3,2]:=  2*(ze_et-xi_ch);a[3,3]:=-xi2-et2+ze2-t-ch2 
end;  {procedure) 


Procedure  Mat_Der(var  xi,  et,  ze,  ch,  wl,  w2,  w3,  xi_,  et_,  ze_,  ch_:seal); 
begin  xi_:=(  ch*wl  -  ze*w2  +  et*w3)/2;  et_:=(  ze*wl  +  ch*w2  -  xi*w3)/2; 
ze_:=(-et*wl  +  xi*w2  +  ch*w3)/2;  ch_:=(-xi*wl  -  et*w2  -  ze*w3)/2  end; 


Procedure  ICsToBFCs(var  x,  y,  z,  x_,  y_,  z_  :seal); 
begin  x:=x_*a[  1 , 1  ]+y_*a[  1 ,2]+z_*a[  1 ,3] ; 
y:=x_*a[2,l]+y_*a[2,2]+z_*a[2,3]; 
z:=x_*a[3,l]+y_*a[3,2]+z_*a[3,3];  end; 
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Procedure  BFCsToICs  (var  x_,y_,z_:seal;  x,y,z:seal); 
begin  x_:=x  *  a[l,l]  +  y  *  a[2,l]  +  z  *  a[3,l]; 
y_:=x  *  a[l,2]  +  y  *  a[2,2]  +  z  *  a[3,2]; 
z_:=x  »  a[l,3]  +  y  *  a[2,3]  +  z  *  a[3,3]  end; 


Procedure  Initialize_Q  (var  xi,  et,  ze,  ch  :seal); 
begin  xi:=0;  et:=0;  ze:=0;  ch:=l;  end; 


Procedure  Integrate(var  a,b,c,d,  uO,ul,  vO,vl,  wO,wl,  xO,xl:seal); 

begin  a:=a+(u0+ul)*dt2;b:=b+(v0+vl)*dt2;c:=c+(w0+wl)*dt2;d:=d+(x0+xl)*dt2 

end; 


Procedure  Up_Date(var  a,b,c,d,e^,g,h:seal);begin  a:=b;c:=d;e:=f;g:=h  end; 


Procedure  Normalize(var  a,b,c,d:seal); 
var  sum:seal;  begin  {Normalize  the  Quateiion  coefficients} 
sum:=  sqrt(  sqr(a)  +  sqr(b)  +  sqr(c)  +  sqr(d) ); 
a:=a/sum;  b:=b/sum;  c:=c/sum;  d:=d/sum  end; 


Procedure  Step(var  channehbyte;  var  value:word;  var  sign:boolean); 
begin 

if  channel  =  0  then  value  :=  value  +  32768; 
if  sign  =  true  then  value  :=  value  +  16384; 
asm  {$G+>  {sccc  cddd  dddd  dddd) 
mov  dx,0300h; 

les  bx, value;  mov  ax,es:[bx];  {ahrdddd  dddd,  al:dddd  dddd} 
out  dx,al;  {dddd  dddd}  {Bits  0  -  7  >  300h} 
movdx,0302h;  {302h>dx} 

mov  al,ah 
out  dx,al 
end; 
end; 


Procedure  Stepper_Driver(  Channehbyte;  var  W:seal;  YO:seal;  var  S0:seal); 

const  alpha  =  2000;  beta  =  100; 

var  sign:boolean;  Wp,a,b:seal;  wrd:word; 

begin 
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WP:=  W; 

W  :={  W  +>  alpha*yO*dt  +  beta*SO; 

if  W  >  0.33  then  W  :=  0.33;  if  W  <  -0.33  then  W  :=  -0.33; 

if  ( (P7In  and  1)  =  0)  then  W  :=  0; 

{  Bound  W.  } 
if  W  <=  Wpthen 

begin  if  W  <  (Wp-0.005)  then  W  :=  Wp-0.005  end 
else 

begin  if  W  >  (Wp+0.005)  then  W  :=  Wp+0.005  end; 

a:=W; 
if  a<  0 

then  begin  sign  :=  false;  a  :=  -a  end 
else  begin  sign  :=  true;  end; 


if  a  >  0.00006  then  b  :=  1/a  else  b 17000; 
wrd  :=  trunc(b); 

if  wrd  >  16383  then  wrd  :=  16383; 

Step  (channel,  Wrd, sign); 
end; 


Procedure  GetPort7(  var  A:  Byte); 

begin  asm  <$G+}  mov  dx,0331h;  in  al,dx;  les  bx.  A;  mov  es:[bx],al  end  end; 


(* 

Procedure  GetPortC(  var  A:  Byte); 

begin  asm  {$G+}  mov  dx,019Ch;  in  al,dx;  les  bx.  A;  mov  es:[bx],al  end  end; 

*) 

Procedure  GetPortB(  var  A:Byte); 

begin  asm  {$G+>  mov  dx,0331h;  in  al,dx;  les  bx.  A;  mov  es:[bx],al  end  end; 


Procedure  SetPort6_l(  var  A:Byte); 

begin  asm  {$G+}  mov  dx,0301h;  les  bx.  A;  mov  al,es:[bx];  {  (A)  >  al} 
oral,bitl;  mov  es:[bx],al;  out  dx,al  end  end; 
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Procedure  ResetPort6_l(  var  A:Byte); 
begin  asm  {$G+)  mov  dx,0301h;  les  bx.  A;  mov  al,es:[bx]; 
and  al,255-bitl;  mov  es:[bx],al;  out  dx,al  end  end; 


Procedure  SetPort6_7(  var  A:Byte); 
begin  asm  {$G+}  mov  dx,0335h;  les  bx,A;  mov  al,es:[bx]; 
or  al,bit7;  mov  es:[bx],al;  out  dx,  al  end  end; 


Procedure  ResetPort6_7(  var  A:Byte); 
begin  asm  {$G+>  mov  dx,0335h;  les  bx,A;  mov  al,es:[bx]; 
and  al,255-bit7;  mov  es:[bx],al;  out  dx,al  end  end; 


Procedure  SetPort6_0(var  ArByte); 

begin  asm  {$G+}  mov  dx,0301h;  les  bx,A;  mov  al,es:[bx];  { (A)  >  al> 
or  al,bitO;  mov  es:[bx],al;  out  dx,al  end  end; 


Procedure  ResetPort6_0(var  ArByte); 
begin  asm  {$G+>  mov  dx,0301h;  les  bx,  A;  mov  al,es:[bx]; 
and  al,255  -  bitO;  mov  es:[bx],al;  out  dx,al  end  end; 


Procedure  Convert(var  srreal;  var  nnriS); 
begin 

{realrsrl  f:39  e:8.  v  :=  (-l)**s*2**(e-129)*(Lf).  if e=0then v:=0} 
{  b47  b46-b8  b7-b0  } 

asm  {MSByte  ah,  al,  ch,  cl  LSByte} 

{$G+} 
les  bxmn 

moval,es:[bx+4]  <  al:  s.in  m.2  m.l  m.O  x  x  b25  b24} 
andal,3  {  al:  0  0  0  0  0  0  b25  b24} 

mov  dl,es:[bx+5]  {  dh:  s.inm.2m.l  m.O  x  x  b27  b26> 
anddl,3  {  dh:  0  0  0  0  0  0b27  b26} 
rol  dl,2 

oral,dl  <  al:  0  0  0  0  b27  b26  b25  b24} 

mov  dl,es:[bx+6]  {  dl:  s.in  m.2  m.l  m.O  x  x  b29  b28> 
and  dl,3 
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rol  (il,4 
or  al,dl 


mov  dl,es:[bx+7]  {  dl:  s.in  m.2  m.l  m.O  x  x  b31  b30> 
and  dl,3 
rol  dl,6 

oral,dl  {al:  b31  b30  b29  b28  b27  b26  b25  b24} 

mov  cl,es:[bx+2]  {cl:  b23  b22  b21  b20  bl9  bl8  bl7  bl6> 
mov  dh,es:[bx+l]  {dh:  bl5bl4bl3bl2bll  blO  b9  b8} 
mov  dl,es:Ibx+0]  {dl:  b7  b6  b5  b4  b3  b2  bl  bO} 
mov ch,0  {ch:  0000000  0} 

test  al, 128  {al:  b31  0  0  0  0  0  0  0) 
jz@00  {Jump  if  negative} 

{sign:l,  al:8,  cl:8,  dh:8,  dl:8, 00:8,  ah:8} 
xor  al,255  {b31  b30  b29  b28  b27  b26  b25  b24} 

xor  cl,255  {b23  b22  b21  b20  bl9  bl8  bl7  bl6} 

xordh,255  {bl5bl4bl3  bl2bll  bl0b09b08} 
xor  dl,255  {b07  b06  b05  b04  b03  b02  bOl  bOO} 
adddl,l;  adcdh,0;  adccl,0;  adcal,0;  movch,128 

@00:mov  ah,128+32  {sign:l,  al:8,  cl:8,  dh:8,  dl:8, 00:8,  ah:8} 
test  al,255;  {Check  for  all  zeros  }  jnz  @3 

mov  al,cl ;  mov  cl,dh ;  mov  dh,dl ;  mov  dl,0 ;  mov  ah,128+8+8+8 
test  al,255;  jnz  @3 

mov  al,cl ;  mov  cl,dh ;  mov  dh,00 ;  mov  ah,128+8+8 

test  al,255;  jnz  @3 

mov  al,cl ;  mov  cl,00 ;  mov  ah,128+8 

test  al,255;  jnz  @3 


mov  al,00 ; 
jmp  @57 

{Finished} 

;  mov  ah,0 

@3: 

test  al,128; 

jnz  @57 

dec  ah; 

test  al,64; 

jnz  @56 

dec  ah; 

test  al,32; 

jnz  @55 

dec  ah; 

test  al,16; 

jnz  @54 

dec  ah; 

test  al,8; 

jnz  @53 

dec  ah; 

test  al,4; 

jnz  @52 

dec  ah; 
dec  ah 

test  al,2; 

jnz  @51 
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;  {al,  cl,  dh,  dl,  00,  ah} 

rcl  dl,l;  rcl  dh,l;  rcl  cl,l;  rcl  al,l 
@51:  rcl  dl,l;  rcl  dh,l;  rcl  cl,l;  rcl  al,l 
@52:  rcl  dl,l;  rcl  dh,l;  rcl  cl,l;  rcl  al,l 
@53:  rcl  dl,l;  rcl  dh,l;  rcl  cl,l;  rcl  al,l 
@54;  rcl  dl,l;  rcl  dh,l;  rcl  cl,l;  rcl  al,l 
@55:  rcl  dl,l;  rcl  dh,l;  rcl  cl,l;  rcl  al,l 
@56:  rcl  dl,l;  rcl  dh,l ;  rcl  cl,l;  rcl  al,l 
@57:  and  al,127;  or  al,ch;  les  bx,s;  mov  es:[bx+5],al 
mov  es:[bx+4],cl;  mov  es:[bx+3],dh;  moves:[bx+2],dl 
xoral,al;  mov  es:[bx+l],al;  moves:[bx+0],ah 
end;  {Asm) 
end;  {Proc  Convert} 


Procedure  Ack_Lo(var  CNT:byte);  begin  asm  {$G+} 
mov  dx,0336h;  mov  ah,0 

@0:  dec  ah;  jz  @  1 ;  in  al,dx;  test  al,128  {bit7 };  jnz  @0 
@1:  les  bx,CNT;  mov  es:[bx],ah  end  end; 


Procedure  Ack_Hi(var  CNT:byte); 
begin  asm  {$G+} 
movdx,0336h;  movah,0 

@0;  decah;  jz@l;  inal,dx;  testal,128  {bit7};  jz@0 
@  1 :  les  bx,CNT ;  mov  es:  [bx]  ,ah  end  end; 


Procedure  Get_Result(var  al,aO:byte); 

begin  asm  {$G+}  mov  dx,0334h;  les  bx,aO;  in  al,dx;  mov  es:[bx],al 
mov  dx,0334h;  lesbx,al;  inal,dx;  mov  es:[bx],al  end  end; 


Procedure  Ext_Sign_Bit(var  aO:byte); 
begin  asm  {$G+}  les  bx,aO;  mov  al,es:[bx]; 
and  al,15;  test  al,8;  jz  @0;  or  al,  240 
@0:  mov  es:[bx],al  end  end; 


Procedure  Int(var  n2;byte;  var  n4,n3:byte); 

begin  asm  {$G+}  les  bx,n2;  mov  al,es:[bx];  mov  ah,al; 
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and  al,112  {Mask  channel  };ror  al,4;  les  bx,n4;  mov  es:[bx],al;mov  al,ah; 
and  al,  12  {Mask  set};  ror  al,2;  les  bx^iS;  mov  es:[bx],al  end  end; 


Procedure  C16(var  countrword;  var  nO,nl:byte); 

begin  asm  {$G+>  les  bx,nO;  mov  al,es:[bx];  les  bx,nl;  mov  ah,es:[bx] 

les  bx, count;  mov  es:[bx],ax  end  end; 


Procedure  get(var  nO:byte); 

begin  asm  {$G+>  mov  dx,0336h;m  al,dx;les  bx,nO;mov  es:[bx],al  end  end; 


Procedure  GetReading  (var  nn:I88;  var  Count:Word;  var  n4^5  :Byte ); 

var  jdc,n34i2,nl,nO,CNT  ;Byte; 

begin  {1 1} 

error  :=  false; 

j:=0; 

n5:=0;  n2  :=  0; 
while  (j<7)do 
begin  {j2> 
inc(n5); 

k  :=  0; 
n4  :=  0; 
while  (k<3)  do 
begin  {k3} 
inc  (n4); 

if  ((n2  and  128)  =  0)  then 
begin  {0  4} 

setport6_7(Port6); 

ack_hi(CNT);  {Wait  for  ack  to  go  high) 
if  (CNT  =0)  then  exit; 
get_result  (n2^0); 
int(n2,j4c); 
nn[j4c+4]  :=  n2; 
im[jdc  ]  :=  nO 
end  {0  4} 
else 

begin  {14} 

resetPort6_7(Port6); 

ack_lo(CNT);  {Wait  for  ack  to  go  low  } 
if  (CNT  =0)  then  exit; 
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get_result  (ii2,nl); 
int(n2,jjc); 
nn[jJc+4]  :=  n2; 
im|j4c  ]  :=nl ; 
end;  {1  4} 

end;  {kS} 

{if  n4  o  4  then  error  :=  true; } 
end;  {j  2> 

setPort6_7(Port6);  {*} 
resetPort6_7(Port6); 

if  n5  <>  8  then  error  :=  true; 

cl  6(count4m[6,3]4m[7,3]); 

end; 


Procedure  ReadShaftEncoder_Azi  (var  S:seal;var  P:byte;  var  T_Word:  word); 

begin  asm  {$G+}  {Do  not  change  bits  P.l  and  P.O} 

les  bxJP  ;  mov  ah,es:[bx];  and  ah,  Ofch  {ahrOOOO  OOrm} 

mov  dx,301h; 

mov  al,ah;  or  al,bit2;  out  dx,al  {alrOOOO  Olnn}  {Set  Update  Command} 
{Delay  for  1  us} 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

les  bx,t_word; 

mov  al,ah;  out  dx,al  {al:0000  OOrm}  {Reset  Update  Command} 
mov  dx,331h 

@0:  in  al,dx  ;  and  al,  bit6 ;  jz  @0;  {Wait  for  update  complete} 

mov  dx,301h  {Set  address  0} 

mov  al,ah  ;  or  al,  bit4  ;  out  dx,al  {al:0001  OOrm} 

{Delay  for  1  us} 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 
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mov  dx,332h  ;  in  al,dx ;  mov  es:[bx],al ;  {Read  LSByte) 

mov  dx,  301h  {ReSet  address  0} 

mov  al,ah  ;  out  dx,al  {al:0000  OOnn} 

{Set  address  1 } 

oral,bit3  ;  outdx,al  {al:0000  lOim} 

{Delay  for  1  us} 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

mov  dx,332h  ;  inal,dx;  mov  es:[bx+l],  al ;  {Read  MSByte) 

mov  dx,  301h  {Reset  Address  1  > 

mov  al,ah  ;  out  dx,al  {al:0000  OOnn) 

end;  {asm) 

S  :=  t_word ; 
end;  {procedure) 

Procedure  ReadShaftEncoder_Ele  (var  S:seal;var  P:byte;  var  T_Word:word); 

begin  asm  {$G+)  {Do  not  change  bits  P.l  and  P.O) 

les  bx,P  ;  mov  ah,es:[bx];  and  ah,  Ofch  {ah:0000  OOnn) 

movdx,301h; 

mov  al,ah;  or  al,bit5;  out  dx,al  {alrOOOO  Olnn)  {Set  Update  Command) 
{Delay  for  1  us) 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

les  bx,t_word; 

mov  al,ah;  out  dx,al  {al:0000  OOnn)  {Reset  Update  Command) 
mov  dx,331h 

@0:  in  al,dx  ;  and  al,  bit?  ;  jz  @0;  {Wait  for  update  complete) 

mov  dx,301h  {Set  address  0} 

mov  al,ah  ;  or  al,  bit?  ;  out  dx,al  {al:0001  OOim) 

{Delay  for  1  us) 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 
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mov  cix,330h  ;  inal,dx;  mov  es:[bx],al ;  {ReadLSByte} 


mov  dx,  301h  {ReSet  address  0} 

moval,ah  ;  outdx,al  {alrOOOO  OOnn} 

{Set  address  1 } 

or  al,bit6  ;  out  dx,al  {al:0000  lOim} 

{Delay  for  1  us> 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop;nop; 

mov  dx,330h  ;  inal,dx;  moves:[bx+l],  al;  {ReadMSByte} 

mov  dx,  301h  {Reset  Address  1 } 

moval,ah  ;  outdx,al  {alrOOOO  OOnn} 

end;  {asm} 

S  :=  t_word ; 
end;  {procedure} 


Procedure  Wheel_Pulse; 
begin  asm  {$G+}mov  dx,0306h; 

mov  al,0;out  dx,al;mov  al,bitO;out  dx,al  end  end; 


Procedure  Wheel_Read(var  Port9_In:Byte); 
begin  asm  {$G+}  les  bxd*ort9_In;  mov  dx,0305h; 
in  al,dx;  mov  es:[bx],al  end  end; 


Procedure  Sensors  (var  wx,wz,wy,joy_o,joy_p,  c_azi,  c_ele  :seal; 
var  Countrword); 

varjjcrbyte; 

begin 

GetReading  (  nn,  Count,  n4,n5  ); 
j  :=  3;  if  (error=false)  then  begin 

for  k  :=  0  to  7  do  nnn[k]  :=  nn[jdc];  Convert(z2zz,nnn)  end;wx:=-zzzz; 
j  :=  7;if  (error=false)  then  begin 

for  k  :=  0  to  7  do  nnnPc]  :=  nnjjjc];  Convert(zzzz,nnn)  end;wz:=-zzzz; 
j  :=  6;  if  (error=false)  then  begin 
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for  k  :=  0  to  7  do  imnDc]  :=  nn|j\k];  Convert(zzzz,mm)  end;wy:=-zzzz; 
j  :=  2;  if  (error=false)  then  begin 

for  k:=0  to  7  do  nnn[k]:=nn|j  Jc];  Convert(zzzz,nnn)  end;joy_0  :=  zzzz; 


j  :=  4;  if  (error=false)  then  begin 

for  k:=0  to  7  do  nnnM:=nn[jJk:];  Convert(zzzz,nnn)  end;c_azi  :=  zzzz; 


j  :=  0;  if  (error=false)  then  begin 

for  k:=0  to  7  do  nnn[k]:=nn[j,k];  Convert(zzzz,nnn)  end;c_ele  :=  zzzz; 


j  :=  5;if  (error=false)  then  begin 

for  k:=0  to  7  do  nnnDc]:=nn[jdc];  ConvertCzzzz^mn)  end;joy_P  :=  zzzz; 
end; 


Procedure  Draw(x,y:seal); 
var  a,b:integer;  c:boolean; 
begin 

c:=false;  G_Set(ApP,BpP.Q; 

A  :=-Trunc(X*l  17)+255;  B  :=  Trunc(Y*100)+255;  G_Set(a,b,c); 

ApP  :=  a;  BpP:=  b; 

end; 


Procedure  Fit(varZZZ,2Z,Z,YZ,SZ:seal); 
var  Y9:  seal; 

begin  YZ  ;=(5*z+zz+zz-zzz)/6;  y9  :=  (z+zz+zzz)/3;  SZ  :=  (YZ  -  Y9)  end; 


(*  Procedure  GetPort77(var  a, b:  byte); 
begin 

a:=0;  b:=255; 
while  aob  do 

begin  asm  {$G+}mov  dx,0331h;  les  bx,b;  in  al,dx;  mov  es:[bx],al; 
les  bx,a;  in  al,dx;  mov  es:[bx],al  end  end  end; 

*) 
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Procedure  Encoders(var  SaeO,  SaeP:Seal); 
begin 

ReadShaflEncoder_Azi(  SaeO,  Port6,temp_word);  {0  <=  SaeO  <=  65535} 
ReadShaftEncoder_Ele(  SaeP,  Port6,temp_word);  <0  <=  SaeP  <=  65535) 
saeo:=  saeo  +  saeo_c;  saep  :=  saep  +  saep_c; 

if  saeO>32767  then  saeo:»=saeo-65535;  if  saep>32767  then  saep:=saep-65535; 
SaeO  :=  SaeO*ConversionToRadians;  SaeP  :=  -SaeP*ConversionToRadians 
end; 


(* 

Procedure  GetPortD(var  value, sain:byte); 

begin  sam:=0;value:=255;while  sam  o  value  do  begin  asm  {$G+}mov  dx,019dh; 
les  bx,value;in  al,dx;  mov  es:[bx],al;les  bx,sam;in  al,dx;  mov  es:[bx],al 
end  end  end; 

♦) 

(*  Procedure  PutPortC(nnnn:byte;var  value:byte);  begin  value:=nnnn; 

asm  {$G+}  mov  dx,019ch;  les  bx, value;  mov  al,es:[bx];  out  dx,al  end;  end; 

*) 

(*  Procedure  BitGet(var  a,b:b)rte); 
begin  asm  {$G+} 

les  bx,B;  mov  al,  es:[bx];  and  al,40h;  add  al,al;  mov  ah,al 
lesbx,A;moval, es:[bx];  oral, ah;  mov es:[bx],al end; end; 

*) 


Procedure  SerialToReal(var  s:arl28b;var  v:real); 
begin 

asm  {$G+>  les  bx,S;  mov  ch,es:[bx  ];  mov  cl,es:[bx+l];  mov  dh,es:[bx+2]; 
mov  dl,es:[bx+3];  mov  ah,es:[bx+4];mov  al,es:[bx+5]; 
les  bx,V;  mov  es:[bx  ],ch;  mov  es:[bx+l],cl;  mov  es:[bx+2],dh; 
mov  es:[bx+3],dl;  mov  es:[bx+4],ah;mov  es:[bx+5],al;end;  end; 


Procedure  ConvertSing(n:word;  var  s:arl28b;  var  x:real); 
var  mJk:word;  tarl28b; 

begin  m:=l;  for  k  :=  n  to  n+1  do  begin  t[m]:=s[k+k-l];inc(m);t[m]:=s[k+k]; 
inc(m);  end;  t[6]:-t[4];t[5]:=t[3];  t[4]:=t[2];t[3]:=0;  t[2]:=0; 
SerialToReal(t,x);  end; 
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Procedure  ConvertReal(n:word;  var  s:arl28b;var  x:real); 

var  mJc:word;  t:arl28b; 

begin  m:=  1;  for  k  :=  n  to  n+2  do 

begin  t[m]  :=  s[k+k-l];  inc(m);  t[m]  :=  s[k+k];  inc(m); 
end; 

serialToReal(t,x) 

end; 


Procedure  PutPort6(var  a:byte); 

begin  asm  <$G+}  mov  dx,0301h;les  bx,a;  mov  al,es:[bx];  out  dx,al;  end  end; 


Procedure  Delay_(a:word); 
var  i:word; 

begin  for  i  :=  0  to  a  do  begin  end  end; 


Procedure  SuperElevation(var  Range,  Angle:  Seal); 

{Units  are  meters  and  radians) 

begin  Angle  :=  Range*(0.000003119+Range*0-00000001521)/2  end; 

(* 


Procedure  LaserRangeFinder, 
begin 

begin 

Port6  :=  Port6  and  (255-32); 

PutPort6(Port6); 

Delay_(200); 

Port6  :=  Port6  or  32; 

PutPort6(Port6); 

Port7Bit6  :=0; 
while  Port7Bit6  =  0  do 
begin 

GetPort7(Port7hi); 

Port7Bit6  :=  Port7In  and  64; 
if  keypressed=true  then  Port7Bit6:=l; 
end; 

GetPortC(PortCO); 
port6 port6  and  255  -  8; 

PutPort6(  Port6 ); 
delay_(400); 

GetPortC(PortCl); 
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Port6  :=  port6  or  8; 
dist  :■=  0; 

if  portcO  and  128  >  0  then  dist:=  dist  +  200; 
ifportcOand  64  >  0  then  dist:=  dist  +  2000; 
if  portcO  and  32  >  0  then  dist:=  dist  +  4000; 
if  portcO  and  16  >  0  then  dist:=  dist  +  8000; 
if  portcO  and  4  >  0  then  dist:=  dist  +  400; 
if  portcO  and  2  >  0  then  dist:=  dist  +  800; 
if  portcO  and  1  >  0  then  dist:=  dist  +  1000; 

if  portcl  and  64  >  0  then  dist:=  dist  +  20; 
if  portcl  and  32  >  0  then  dist:=  dist  +  40; 
if  portcl  and  16  >  0  then  dist:=  dist  +  80; 
if  portcl  and  8  >  0  then  dist:=  dist  + 100; 
if  portcl  and  2  >  0  then  dist:=  dist  +  5; 
if  portcl  and  1  >  0  then  dist:=  dist  +  10; 
range  :=  dist  +  0.1; 
if  range  <  5  then  range  :=  5; 
end;  {Reading  the  Laser  Range  Finder} 

end; 

*) 


Procedure  Time_Of_Flight(var  a,  b:  seal); 

begin  a;=  b*(0.00092314  +  b*0.00000109913)  end;  {b  denotes  the  range} 


Procedure  Matrixhitegrate;  begin 

Mat_Der(  xi,  et,  ze,  ch,  wx,  wy,  wz,  xidp,  etdp,  zedp,  chdp); 
Integrate(xi,et,ze,ch,  xidO, xidp,  etdO, etdp,  zed0,zedp,  chd0,chdp); 
Up_Date(xid0,  xidp,  etdO,  etdp,  zedO,  zedp,  chdO,  chdp); 
Nonnalize(xi,  et,  ze,  ch);  {Assures  orthonormality} 
if  WC  then  Wheel_Pulse;  {Allows  time  for  the  embedded  controller} 
Mat(  xi,  et,  ze,  ch,  A);  {Defines  the  A  transformation  matrix}  end; 


Procedure  DriftCorrection;  begin  wx  :=  Coeffl  *  (wx/count  -  DriftX); 
wy:=Coeffl*(wy/count  -  DriftY);  wz  ;=  Coeffl  *  (wz/count  -  DriftZ);  end; 


Procedure  DAC00(var  xxrinteger);  begin  (*asm  {$G+} 

mov  dx,0ffe0h;  les  bx,xx;  mov  ax,es:[bx];  xor  ah,008h 
out  dx,ax  end*)  end; 

Procedure  DAC01(var  xxrinteger);  begin(*  asm  {$G+} 

mov  dx,0ffe2h;  les  bx,xx;  mov  ax,es:[bx];  xor  ah,008h 


40 


out  dx,ax  end*)  end; 


Procedure  DAC02(var  xx:  integer);  begin  (*asm  {$G+} 

mov  dx,0ffe4h;  les  bx^;  mov  ax,es:[bx];  xor  ah,008h 
out  dx,ax  end*)  end; 

Procedure  DAC03(var  xx:  integer);  begin  (*  asm  {$G+} 

mov  dx,0ffe6h;  les  bx,xx;  mov  ax,es:[bx];  xor  ah,008h 
out  dx,ax  end*)  end; 

Procedure  ConfigA(var  z:byte);  { J3  } 
begin  asm{$G+} 

mov  dx,0303h;  {Configuration  A}les  bx,z;mov  al,es:[bx];out  dx,al  end  end; 

Procedure  ConfigB(var  z:byte);  {J4} 
begin  asm{$G+} 

mov  dx,0307h;  {Configuration  B>les  bx,z;mov  al,es:[bx];out  dx,al  end  end; 

Procedure  ConfigC(var  z:byte);{J3} 
begin  asm{$G+} 

mov  dx,0333h;  {Configuration  C}les  bx,z;mov  al,es:[bx];out  dx,al  end  end; 

Procedure  ConfigD(var  z:byte);{J4} 
begin  asm{$G+} 

mov  dx,0337h;  {Configuration  D}les  bx,z;mov  al,es:[bx];out  dx,al  end  end; 


(* - MAIN- - *) 

{  The  origin  for  the  Body  Fixed  Coordinates  (BFCs)  is  the  sight 
aligned  with  the  orientation  of  the  buggy. 

XX,  yy  and  zz  define  the  location  of  the  sight  in  Inertial  Coordinates  (ICs). 
xw,  yx  and  zw  define  the  location  of  the  counter  wheel  in  ICs. 
xb,  yb  and  zb  define  the  location  of  die  target  wrt  the  BFCs. 
xa,  ya  and  za  define  the  location  of  the  target  wrt  the  ICs. } 

BEGIN  {main}  G_Init;  {Initializes  the  video  display  board} 

ww:=$80;ConfigA(ww); 
ww:=$82;ConfigB(ww); 
ww:=$9b;ConfigC(ww); 
ww:=$99;ConfigD(ww); 
app:=0;bpp:=0;  {Initializes  the  graphics} 

Port6  :=127; 

GetPort7(P7hi); 
writeln('main '); 
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GPS:=FALSE;  WC:=true;{  if((P7In  and  4)=0)  then  GPS  :=  TRUE  else  WC  :=  TRUE;} 

ao  :=  0;  ap  :=  0;  {Dummy  variables  used  by  the  stepper  procedure) 

Jo5G_:=0;  JoyP_:=0;  {Used  in  the  joystick  integration  mode) 

C_Joy_0:=pi/(l  80* 131 072);C_Joy_P:=pi'(l 80* 131 072);  {Joy  stick  sensitivity} 

Hop:^l;  SaeO_:=  0;  SaeP_  :=  0;  OO:=0;  O:=0;  PP:=0;  P:=0; 

sensors(wy,wz,wx  joyp ,joyo,c_azi,  c_ele,  Count);  {Done  for  synchronization} 

c_a_drift  :=  c_azi/count;  c_e_drift:=  c_ele/count; 

get_result  (n24i0);  ii_c_azi:=0; 

Ack_Lo(CNT);  {Test  slave's  output  -  low  normal  state} 
if(CNT  =  0)then 
begin 

writeln(  'Slave  in  wrong  state.  Press  any  key'); 
halt; 
end; 

writeln('sensors '); 

suml  :=  0;  sum2  ;=  0;  B1  :=  0;  B2  :=  0; 

ResetPort6_0(Port6);  Fir  :=  False;  IXL:=0; 

xc:=0;  dt  :=  0.0060;  dt2  :=dt/2; 
xid0:=0;  etd0:=0;  zed0:=0;  chd0:=0; 

Speed:=0; 

Range  :=  100;  {Center  of  Screen  (COS)  Sight  to  Target  distance} 

{xb,  yb,  zb  define  the  target  in  Body  Fixed  Coordinates  (BFCs)} 

Encoders  (SaeO,SaeP);  {Center  of  the  Screen}  {O  elevation,  P  azimuth} 

CosP  :=  cos(SaeP);  SinP  :=  sin(SaeP);CosO  :=  cos(SaeO);  SinO  :=  sin(SaeO); 

xb:=CosO*CosP*Range; 

yb:=CosO*SinP*Range; 

zb:=SinO*Range;{COS,  wrt  BFCs,  BFCs} 

if  WC  then 
begin 

Wheel_Pulse;  {Initializes  wheel  pulse  counter} 
xx:=0;yy:=0;zz:=0;  {xx  yy  zz:location  of  the  origin  of  the  BFCs,  ICs} 
end; 
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Iiiitialize_Q(xi,et,ze,ch);  {0,0,0,1> 

Mat(  xi,  et,  ze,  ch.  A);  {Defines  the  A  transformation  matrix,  A=ll  J  > 

{Initial  position  ofthe  Reference  Wheel,  ICs>{A=[l]}  v  r,- 

xw:*xx  -  WheelToSight_X;  yw:=yy  -  WheelToSight_Y;  zw:=zz-WheelToSight_Z; 
{xw  yw  zw:  the  Reference  Wheel  wrt  BFCs,  ICs} 

SuperElevation(Range,  SE);  {SE  defines  the  super  elevation  angle} 

ZSE  :=  SE*Range;  {ZSE  becomes  the  apparent  z  offset  of  the  target} 

BFCsToICs(xa,ya,za,xb,yb,zb);  {Target's  Position,  COS,  wrt  BTCs,  ICs} 
xxx;=xa+xx;  yyy:=ya+yy>  zzz:=za+zz;  {COS&Range,  wrtICs,ICs} 

IntP:=0;  IntO:=0;  yo:=0;  yp:=0;  so:=0;  sp:=0; 

i_c_azi;=0;  i_c_ele:=0;  {Integrals  of  the  camera  rates,  azi.  and  ele. } 

writehi('xx  yy  zz',  xx:  1 2:2,  yy;  12:2,  zz:12:2); 
writeln('xb  yb  zb',  xb:12:2,  yb:12:2,  zb:12:2), 
writeln('xa  ya  za',  xa:12:2,  ya:12:2,  za:12:2); 

writelnC'xxxyyy  zzz ',  xxx:  12:2,  yyy:  12:2,  zzz:  12:2); 

Looper  :=  0;  {Used  by  the  wheel  coimter  to  automatically  go  to  drift  mode} 
writelnCEntering  master  loop'); 

l! 

for  111:=  1  to  40  do 
bcsin 

sensors(wy,wz,wx,joyp,joyo,c_ele,  c_azi.  Count);  {Done  for  synchronization} 
c_a_drift  :=  c_azi/count; 
c_e_drift:=  c_ele/count; 

driftx:=wx/count;  drifty:=wy/count;  driftz:=wz/count; 
c_a_drift  :=  c_a_drift  +  ( c_azi  -  c_a_drift)*0.001 ; 
c_e_drifl  :=  c_e_drift  +  ( c_ele  -  c_e_drift)*0.001; 
driftx:=driftx+(wx-driftx)*0.001 ; 
drifty:=drifty+(wy-drifty)*0.001; 
driftz:=driftz+(wz-driftz)*0.001; 

end; 

sensors(wy,wz,wx,joyp,joyo,c_ele,  c_azi.  Count);  {Done  for  synchronization} 
driftx:=  wx/count;  di^:=wy/count;  driftz:=wz/count; 
c_azi:=c_azi/count;  c_ele:=c_ele/  count; 

driftx:=41;drifty:=-753;driftz:=356  ; 
c_a_drift:=2157;  c_e_drift:=574; 

^xx  :=  0;  vyy  :=  0  {-  4.47}  {10  mph};  vzz:=0;  {Speed  of  the  target  in  BFCs.} 


t:=0; 

weppp:=0;  wepp:=0;  wep:=0; 
wappp:=0;wapp:=0;  wap:=0; 


clrscr; 

(* - - -  Master  Loop - - - *) 

while  true  do 
begin 

if  keypressed=true  then 

begin  writeln(driftx:  1 6:6,drifty:  1 6:6, driftz:  16:6, 
c_a_drift:16:6,c_e_drift:16:6);  halt  end; 

GetPort7(P71n); 

Sensors(wy,  wz,  wx,  JoyP,  JoyO,  c_ele,  c_azi.  Count); 

c_azi  :=  c_azi/count;  c_ele  :■=  c_ele/count; 
c_azi  :=  c_azi  -  c_a_drift;  c_ele  :=  c_ele  -  c_e_drift; 


DriftCorrection;  {Cancels  out  static  drift  in  wx,  wy  and  wz} 

;  {Looper  is  used  to  turn  on  and  off  the  drift  corection  > 

{and  is  based  on  when  the  last  wheel  pulse  was  detected. } 

if  ((Looper=0)  and  (P7Ih  and  1=0))  then 
begin 

if  wx  <  0  then  driftx  :=  driftx  -  0.2  else  driftx  :=  driftx  +  0.2; 
if  wy  <  0  then  drifty  :=  drifty  -  0.2  else  drifty  :=  drifty  +  0.2; 
if  wz  <  0  then  driftz  :=  driftz  -  0.2  else  driftz  :=  driftz  +  0.2; 
end; 

if  c_azi<0  then  c_a_drift  :=  c_a_drift  - 1.2  else  c_a_drift  :=  c_a_drift  +  1.2; 
if  c_ele<0  then  c_e_drift  :•=  c_e_drift  -  1.2  else  c_e_drift  :=  c_e_drift  +  1.2; 

Matrixintegrate;  {Update  Quaterions  based  on  angular  rates} 

if  WC  then 
begin 

Wheel_Read(Port9_In);  {Number  of  pulses  since  last  update} 

Temp  :=  Port9_hi*0.133;  {0.133  is  the  distance  in  meters  per  count} 
BFCsToICs(xx,yy,zz,WheelToSight_X,WheelToSIght_Y,WheelToSight_Z); 
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xw  :=  xw  +  Temp*a[l,l];xx:=  xx  +  xw;  {xw:  Ref.  Wheel's  position,  ICs> 
yw  :=  yw  +  Temp*a[l,2];yy:=  yy  +  yw;  {xx:  Scope's  position,  ICs} 
zw  :=  zw  +  Temp*a[l,3];zz:=  zz  +  zw; 

{xw,  yw  and  zw:  the  Reference  Wheel's  position  in  IC} 

{xx,  yy  and  zz:  the  location  of  the  sight  (origin  of  BFCs)  in  ICs} 
if  Port9_In  >  0  then  looper:=1815  else  if  looper>0  then  dec(looper); 
Speed:=  0.99*Speed  +  0.01*temp/dt; 
end; 

{  gotoxy(l,l);  write(speed:12:2);  } 


Encoders  (SaeO,SaeP);  {Center  of  the  Screen)  {O  elevation,  P  azimuth) 
CosP  :=  cos(SaeP);  SinP  :=  sin(SaeP);CosO  :=  cos(SaeO);  SinO  :=  sin(SaeO); 


{Routine  for  setting  the  Range) 

GetPortB(PortB_In); 

If  (PortB_In  and  4=0)  then 

begin  {manual  ranging  and  Target  Position  (BFCs)  update) 

IntO:=C_Joy_0*JoyO/count;  IntP  :=-C_Joy_P*JoyP/count; 
i_c_ele:=0;  i_c_azi:=0;  {Terms  used  for  the  joystick  integration) 
xw:=0;  yw:=0;  zw:=0; 

If  ( (PortB_Ih  and  8)  =  0 )  then  Range  :=  Range  +  0.1; 

If  ( (PoitB_hi  and  16)  =  0 )  flien  Range  :=  Range  -  0.1 ; 
if  range  <  5  then  range  :=  5; 

xb:=  CosO*CosP*Range ;  yb  :=  CosO*SinP*Range;  zb  :=  SinO*Range; 
BFCsToICs(xa,ya,za,xb,yb,zb); {Center  of  Screen  (COS)  &  Range,  wrt  BFCs) 
xxx:=xa+xx;yyy:=ya+yy;zzz:=za+zz;{COS&Range,  wrt  ICs,ICs) 
t:=0; 

end; 

{gotoxy(l,l); 

writeln(xxx:  12:2,yyy:  12:2,zzz:  12:2); 
writeln(  xx:12:2,  yy:12:2,  zz:12:2); 

) 


{Target  Acquire) 

if((PortB_In  and  4>0)  and  ((PortB_In  and  16)=0))  then 
begin 

{  LaserRangeFinder;) 
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if  range  <10  then  range:=100; 

IhtO:=  C_Joy_0*JoyO/count;  IhtP  :=  -C_Joy_P*JoyP/count; 
i_c_ele:=0;  i_c_azi:=0;  {Terms  used  for  the  joystick  integration} 
xw:=0;  yw:=0;  zw:=0; 

xb  :=  CosO*CosP*Range;  yb  :=  CosO*SinP*Range;  zb  :=  SmO*Range; 
BFCsToICs(xa,ya,za,xb,yb,zb); 

xxx:=xa+xx;  yyy:=ya+yy;  zzz:=za+zz;{COS&Range,  wrt  ICs,ICs> 
sensors(weO,wel  ,we2,we3,we4,we5,we6,  we7); 

t:=0; 

end; 

{  t:=t  +  dt;> 

{  XXX  :=  XXX  +  vxx*dt;  yyy  :=  yyy+vyy*dt ;  zzz  :=  zzz+  vzz*dt;  > 

{gotoxy(l,l);  writeln(t:12:2,xxx:  12:2,  yyy:  12:2, zzz:  12:2);  > 

RRR:=XXX  -  XX;  SSS:=YYY  -  YY;  TTT:=ZZZ  -  ZZ;  {COS&Range,  wrt  BFCs,  ICs> 
Range:=sqrt(sqr(RRR)  +  sqr(SSS)  +  sqr(TTT));  {COS  distance) 

{  Output(4,Range);  } 

Time_Of_Flight(TimeOfFlight,Range); 

{TimeOfRight  is  an  approximation  neglecting  the  speed  of  the  vehicle) 

ICsToBFCs(ir,ss,tt,rrr,sss,ttt);{COS&Range+Gravity,  wrt  BFCs  (BFCs&ICs)) 
aziO  :=  atan(SSJRR);  eleO  :=atan(TT,sqrt(sqr(RR)+sqr(SS)));  {COS) 

9 

SuperElevation(Range,  SE);  {SE  defines  the  super  elevation  angle) 

ZSE  :=  SE*Range;  {ZSE  becomes  the  apparent  z  offset  of  the  target) 

9 

{  dxx:=vxx*TimeOfFlight ;  dyy:=  vyy*TimeOfFlight;  dzz:=vzz*TimeOfFlight; } 

{  ICsToBFCs(dx,dy,dz,dxx,dyy,dzz);) 


RR:=RR  +  Speed*TimeOfHight  {-  DX);  {RR  SS  TT:  hit  point,  wrt  BFCs,  BFCs) 
{  SS  :=  SS  -  DY;  ) 

TT:=TT  -  ZSE  {-  DZ) ;  {Gravity  Drop  of  the  Projectile) 

{C0S&Range4Gravity,  Lead  Angle,  wrt  BFCs) 


azil:=atan(SS,RR){+0.238/range);  elel  :=atan(TT,sqrt(sqr(RR)+sqr(SS))); 
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SetPort6_0(Port6); 

{20  }  Stepper_Driver(0,AO,Y  O+0.5*SO,SO); 

Slew  :=  0.005;  if  ((PortB_In  and  64)=0)  then  slew  :=  0.05; 

JoyP  :=  C_Joy_P*JoyP/count; 

if  ((P7in  and  2)=0)  then  IntP  :=  IhtP  +  JoyP*slew; 

JoyP_  :=  JoyP  +  IntP; 

Slew  :=  0.005;  if  ((PortB_hi  and  8)=0)  then  slew  :=  0.01; 

JoyO  :=  -  C_Joy_0*JoyO/count; 

if  ((P7in  and  2)=0)  then  IntO:=IntO  +  JoyO  *  slew  {else  t:=0}; 

JoyO_  :=  JoyO  +  IntO; 

{2P}  Stepper_Driver(l  ,AP,YP+0.5*SP,SP); 

SaeO  :=  SaeO  +  JoyO_;  {Elevation)  Saep  :=  Saep  +  JoyP_;  {Azimuth} 
D_E10  :=(SaeO  -  EleO);  D_Az0 (SaeP  -  AziO); 

D_E11  :=(SaeO  -  Elel);  D.Azl  :=  (SaeP  -  Azil); 

{Elevation,  O,  Azimutii,P> 

000  :=  OO;  00;=0;  O  :=-D_E10;  PPP  PP;  PP:=P;  P  :=  D_AzO; 
Y09:=Y0;  Fit(000,00,0,Y0,S0) ;  {Generates  YO,SO> 

YP9  :=  YP;  Fit(PPPd>PJ>,YP,SP);  {Generates  YP,SP} 

{00}  Stepper_Driver(0,AO,YO,SO); 

ele_lim:=0.0030;  azi_lim;=0.0030;  tau:=0.4; 


conl:=dt;  con2:=l-conl; 

c_azi: =c_azi*0.0000 1 0; 
c_ele:=c_ele*0.000010; 


i_c_ele  :=  (i_c_ele  +  c_ele  *conl)*con2; 
i_c_azi  :=  (i_c_azi  +  c_azi  *conl)*con2; 


{gotoxy(l,l);  write(c_azi:12:6,  i_c_azi:  12:6, c_a_drift:  12:6);} 

if  i_c_ele  >  ele_lim  then  i_c_ele  :=  ele_lim; 
if  i_c_ele  <  -ele_lim  then  i_c_ele  :=  -ele_lini; 
if  i_c_azi  >  azi_lim  then  i_c_azi  :=  azi_lim; 
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if  i_c_azi  <  -azi_lim  then  i_c_azi  :=  -azMim; 


azza:=  -  i_c_azi; 
azze:=  i_c_ele; 

{writeC  azzal  ',azza:8:4);  } 

weppp:=wepp;  wepp:=wep;  wep:=  -D_E11; 
wappp:=wapp;wapp:=wap;  wap:=  D_Azl; 


draw(  weppp  +  azze , 
wappp  +  azza  ); 


for  m  :=  1  to  5  do  r[m-l]:=r[m]; 
r[5]:=o; 

john:=r[5]+(r[5]-r[0]); 

for  m  :=  1  to  5  do  v[m-l]:=v[m]; 
v[5]:=p; 

jane  :=  v[5]+(v[5]-v[0]); 


FirP  :=  Fir; 

If  ( (P7In  and  32=0)  and  (Fir=False) )  then 

begin  eir:=sqrt(sqr(john)+sqr(Jane));  if  eiT<0.0002  then  Fir:=True  end; 

if  ((Fir  =  True)  and  (FirP  =  False))  then  SetPort6_l(Port6); 
if  (Fir  =  True)  then  inc(LLJL); 
if  (LLL=  10)  then  ResetPort6_l(Port6); 

if  (LLL  =  100)  then  begin  Fir:=False;  ResetPort6_l(Port6);LLL:=  0;  end; 


if  fir  then  John  :=  john+  0.002; 
if  fir  then  jane  :=  jane  +  0.002; 


if  (o  >  ANG)  then  xc:=$7ff  else  if  (o  <  -0.05/3)  then  xc:=$800  else 
xc  :=  trunc(  temp  *  40960*3);  DACOO(xc); 


{  ***  }  temp2:=  -  weppp{-  c_azi*0.002>; 

if  (p  >  ANG)  then  xc:=$7ff  else  if  (p  <  -0.05/3)  then  xc:=$800  else 
xc:=trunc(  temp2  *40960*3  );  DAC01(xc); 
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if  (john  >  ANG)  then  xc:=$7ff  else  if  (John  <  -0.05/3)  then  xc:=$800  else 
xc  :=  trancO'ohn*40960*3);  DAC02(xc); 

if  (jane  >  ANG)  then  xc:=$7ff  else  if  (jane  <  -0.05/3)  then  xc:=$800  else 
xc  :=  trunc(jane*40960*3);  DAC03(xc); 


GetPort7(P7hi); 

P7InP:=P71n; 

(*  while  (  not  (P7hi  and  8  =  8)  and  (P7InP  and  8  =  0) )  do 
begin  P7InP:=P7In;GetPort7(P7In)  end;  {External  Clock  Synchronization) 

*) 

ResetPort6_0(Port6); 

<0P}  Stepper_Driver(l,  AP,YP,SP); 
end; 

end.  {main} 
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